/*
 * Guidance.h
 *
 *  Created on: Jun 9, 2011
 *      Author: tflanzer
 */

#ifndef GUIDANCE_H_
#define GUIDANCE_H_

#include <cstdio>
#include <stdio.h>
#include <list>
#include <math.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <string.h>
#include <complex>
#include "Matrix.h"
#include "FileWriter.h"
#include "execStruct.h"
#include "Communication.h"

#define NB 100 // Max number of bezier curves
#define ds 25 // Discretization used for fitting
#define trajType "circle" // Trajectory type (circle,
                                    // straight_north, racetrack,
                                    // racetrack_SWARMS)

class Guidance: public Communication::GNDListener {
public:

	double wtm[3][2*NB];
	double wp[3][NB+1], wp0[3][NB+1];
	double u, ul, ta, L1, Nc, Ec, Rcirc, dDes, altDev, vDev, Vtarget;
	int wpl, fgnd;
	FileWriter *fw;

	Guidance(FileWriter *fw, int fg);
	virtual ~Guidance();

	void GNDdataReceived(Communication &com);
	void genTraj(double R, double *r);
	void getBezierMatrices(double *At, double *bt);
	void getBezierRHS(double *b0, double *bt, double *r);
	void computeWaypointTracks();
	void computeWaypointNumber(double *p, int idx[2]);
	void computeCurvature(int idx[2], double dBdu[3]);
	void findPseudoLoc(double *p, int idx[2], double ploc[3], double ru[3], double nru[3]);
	double getBankAngleCmd();
	void cross(double a[3], double b[3], double c[3]);
	void loadGains();
	void computeNearestPoint(double tploc[3]);

private:
	void spline(double *x, double *y, double *xi, double *yi, int n, int ni);
	double computeAirspeed();
	void inertial2body(double psi, double th, double phi, double *x);
};

#endif /* GUIDANCE_H_ */
